
#include "control_task.h"	
#include "trait.h"	

void Trot_Leg_Rect(Leg_t* in, double phase){
	double Ax = in->Ax, Ay = in->Ay, zero_x = in->zero_x, zero_y = in->zero_y;

	double x = 0;
	double y = 0;
	double z = 0;
	if (phase > 2*PI) phase = phase - 2*PI;
	double a = 2*PI/2+PI/6;
	
	double b = 2*PI/2+PI/3+PI/6;
	double c = 2*PI/2+2*PI/3+PI/12;
	
	double d = 2*PI;

	if (phase < a){
		x = -Ax * (phase-0) / (a-0);
		y = zero_y;
		Set_Feet_Pos(in->index, x, y, z);
	}
	else if (phase < b){
		x = -Ax;
		y = zero_y - Ay * (phase-a) / (b-a);
		Set_Feet_Pos(in->index, x, y, z);
		
	}
	else if (phase < c)
	{
		x = -Ax + Ax * (phase-b) / (c-b);
		y = zero_y - Ay;
		Set_Feet_Pos(in->index, x, y, z);
		
	}
	
	else if (phase < 2*PI)
	{
		x = 0;
		y = zero_y - Ay + Ay * (phase-c) / (d-c);
		Set_Feet_Pos(in->index, x, y, z);
	}
	
	return;
}

/**
  * @brief  Trot��̬��������
  * @param  in: ��Ҫ������Leg_Index
  * @param  Ax: x�����ϵ�����
  * @param  Ay: y�����ϵ�����
  * @param  zero_x: x�������λ��
  * @param  zero_y: y�������λ��
  * @retval None
  */
double YH = 2;
double YL = -0.5;
double XH = 3;
double XL = -2;
void Trot_Leg(Leg_t* in, double phase)
{
	double Ax = in->Ax, Ay = in->Ay, zero_x = in->zero_x, zero_y = in->zero_y;
	double x = 0;
	double y = 0;
	
	if (phase > 2*PI) phase = phase - 2*PI;
	
	if ((phase<=PI*11.0/12)) //��
	{
		x = ((XH-XL)/2 *( - cos(phase/11*12)))+(XH+XL)/2;
		y = YH*sin(phase/(11.0/12));
		x = zero_x + x*Ax;
		y = zero_y + y*Ay;
		Set_Feet_Pos(in->index,x, y, 0);
	}
	else if ((phase>PI*11.0/12) & (phase<=PI))//���������
	{
		
	}
	else if ((phase>PI) && (phase<=(1+5.0/6)*PI)) //��
	{
		x = ((XH-XL)/2 * cos((phase-PI)*6.0/5)+(XH+XL)/2);
		y = YL*sin(6.0/5.0 /2.0*(phase-PI));
		x = zero_x + x*Ax;
		y = zero_y + y*Ay;
		Set_Feet_Pos(in->index,x, y, 0);
	}
	else if ((phase>(1+5.0/6)*PI) & (phase<=(1+11.0/12)*PI))//���������
	{
		
	}
	else if((phase>(1+11.0/12)*PI)&(phase<=2*PI)) //̧
	{
		x = XL;
		y = -YL*sin(12.0/2*(phase-2*PI));
		x = zero_x + x*Ax;
		y = zero_y + y*Ay;
		Set_Feet_Pos(in->index,x, y, 0);
	}
	
}


void Jump_Leg(Leg_t* in, double phase)
{
	double Ax = in->Ax, Ay = in->Ay, zero_x = in->zero_x, zero_y = in->zero_y;
	double x = 0;
	double y = 0;
	double z = 0;
	
	double a = 2*PI/4;
	double b = 2*PI-2*PI/4;
	if (phase < a)
	{
		x = 0;
		y = zero_y - Ay + Ay * (phase) / (a);
		Set_Feet_Pos(in->index, zero_x+x, y, z);
	}
	else if (phase < b)
	{
		x = -Ax;
		y = zero_y - Ay * (phase-a) / (b-a);
		Set_Feet_Pos(in->index, zero_x+x, y, z);
	}
	
	return;
}
